from Source.CWJ import qyhf
from Methods.tools import *
import copy
from Method import blocks_select
from Avoidance_CURD import Pi_bypass, pilist_avoidance, Set_slope, LinkedList
# from my_plot import *
# from Source.Astar20221013.main import astar
from avoiding_obstacles import *
import math

def coincide_judge(pipe1, pipe2, length):  # 比较起点接下一个终点是否邻近

    if math.sqrt((pipe1[0] - pipe2[0]) ** 2 + (pipe1[1] - pipe2[1]) ** 2 + (pipe1[2] - pipe2[2]) ** 2) <= length:
        return True
    else:
        return False


def get_true_pipe(new_pi):
    pi = copy.deepcopy(new_pi)
    if len(pi) > 1:
        # print(len(new_pi))
        for i in range(len(pi) - 1):
            length = max(pi[i][2], pi[i][3])*1.5
            pipe1 = pi[i][1]
            pipe2 = pi[i + 1][0]
            print(pipe1)
            print(pipe2)
            flage = coincide_judge(pi[i][1], pi[i + 1][0], length)
            print(flage)
            if not flage and i == 0:
                # 改变坐标
                print('第一段管,可以调当前管和下一段管道')
                if coincide_judge(pi[i][0], pi[i + 1][0], length):
                    print('需要对换当前管道起点终点')
                    pi[i][0], pi[i][1] = pi[i][1], pi[i][0]
                if coincide_judge(pi[i][1], pi[i + 1][1], length):
                    print('需要对换下一条管道起点终点')
                    pi[i + 1][0], pi[i + 1][1] = pi[i + 1][1], pi[i + 1][0]
                if coincide_judge(pi[i][0], pi[i + 1][1], length):
                    print('需要对换当前管道和下一段管道起点终点')
                    pi[i][0], pi[i][1] = pi[i][1], pi[i][0]
                    pi[i+1][0], pi[i+1][1] = pi[i+1][1], pi[i+1][0]
                    print('调整后',coincide_judge(pi[i][1], pi[i + 1][0], length))
            elif not flage and i != 0:
                print('只能调下一条管')
                pi[i + 1][0], pi[i + 1][1] = pi[i + 1][1], pi[i + 1][0]
            else:
                print('不需要调整')
    return pi

def distance_to_origin(point):
    return math.sqrt(point[0] ** 2 + point[1] ** 2 + point[2] ** 2)
# 原来管道组排管算法,不含三通管道排管
def pipegroup_main(pipeBase, blockBase, block_arr, PipeGroup_info, new_allpipelist, flag):
    PipeGroupNo = PipeGroup_info[0]
    PipeGroup_Type = PipeGroup_info[1]
    ceilling = PipeGroup_info[2]
    floor = PipeGroup_info[3]
    for PipeGroup in pipeBase._pipeGroups:
        if PipeGroup.pipeGroupNo == PipeGroupNo:
            for pipeList in PipeGroup._pipeLists:
                pilist = PipeListNo_to_PipeList(PipeGroupNo, pipeList.pipeListNo, pipeBase)
                if PipeGroup_Type == 'LinePipe' or PipeGroup_Type == 'LineDuct' or PipeGroup_Type == 'Duct':
                    pilist = get_true_pipe(pilist)
                print('旧管道是', pilist)
                copy_pilist = copy.deepcopy(pilist)
                # todo 重新确定管道的起点 1 高的为起点 2 离（0，0）近的为起点
                # 创建 DataAdjuster 实例
                pilist = DataAdjuster(pilist)

                # 调整数据
                pilist= pilist.adjust_data()
                print('调整后', pilist)
                copy_change_pilist = copy.deepcopy(pilist)
                blocks_selection = blocks_select(pilist, block_arr, 5000)
                #  进行距离排序
                blocks_selection = sorted(blocks_selection, key=lambda item: distance_to_origin(item[0]))
                if flag == 0:
                    block_wall_list_recursion = copy.deepcopy(blocks_selection)
                    slope_change_flag, pilist = Set_slope(pilist, PipeGroup_Type, ceilling)  # 为原始管道设置斜率
                    linkedlist = LinkedList()  # 创建双向链表
                    linkedlist.create_LinkedList(pilist)  # 初始化双向链表
                    change_flag1 = pilist_avoidance(linkedlist, blocks_selection, blockBase, PipeGroup_Type, ceilling,
                                                    pipeBase)  # 用于同一列中的管道避障，change_flag1为避障标志

                    new_pipeList = linkedlist.output_Arr()
                    change_flag2, new_pipeList, position_list = Pi_bypass(new_pipeList, blocks_selection, blockBase,
                                                                          block_wall_list_recursion, PipeGroup_Type,
                                                                          ceilling)

                    print('change_flag1', change_flag1)
                    change_flag = change_flag1 or change_flag2
                    print('change_flag', change_flag)
                    new_pipeList = DataAdjuster(new_pipeList)

                    # 调整数据
                    new_pipeList = new_pipeList.reveresd_adjust_data(copy_pilist, copy_change_pilist)
                    # print('新管道是:', new_pipeList)

                    double_update(new_pipeList, pipeBase, PipeGroupNo, pipeList.pipeListNo, position_list, change_flag,
                                  slope_change_flag)  # 将二通管道数组信息更新至管道类及增加连接关系
                    # plot_pipes_walls(pilist,new_pipeList)
                else:
                    new_pipeList = astar(pilist, blocks_selection, 20, 20, floor, ceilling[0], ceilling[1])
                    double_update(new_pipeList, pipeBase, PipeGroupNo, pipeList.pipeListNo, position_list=[[], []],
                                  change_flag=True, slope_change_flag=True)  # 将二通管道数组信息更新至管道类及增加连接关系
                    print('A*算法后的新管道是', new_pipeList)
                # 每生成一列管道，就存进数组中，最后把所有的新管道都存进去
                new_allpipelist.append(new_pipeList)
                new_blocklist = qyhf.pipes_to_blocks(new_pipeList, 15)
                # # 管道类型的障碍物，添加到障碍物对象中
                add(new_blocklist, blockBase)
                # # 管道类型的障碍物，添加到障碍物数组中
                block_arr.extend(new_blocklist)


# 水管三通排管
def pipelist_main(flag, pilist, block_arr, blockBase, PipeGroup_Type, ceilling, pipeBase):
    blocks_selection = blocks_select(pilist, block_arr, 5000)
    block_list_leave = copy.deepcopy(blocks_selection)
    print('旧管道是', pilist)
    slope_change_flag, pilist = Set_slope(pilist, PipeGroup_Type, ceilling)  # 为原始管道设置斜率
    linkedlist = LinkedList()  # 创建双向链表
    linkedlist.create_LinkedList(pilist)  # 初始化双向链表
    change_flag1 = pilist_avoidance(linkedlist, blocks_selection, blockBase, PipeGroup_Type, ceilling,
                                    pipeBase)  # 用于同一列中的管道避障
    new_pipeList = linkedlist.output_Arr()
    change_flag2, new_pipeList, position_list = Pi_bypass(new_pipeList, blocks_selection, blockBase, block_list_leave,
                                                          PipeGroup_Type, ceilling)
    change_flag = change_flag1 or change_flag2
    print('新管道是:', new_pipeList)
    return change_flag, slope_change_flag, new_pipeList, position_list


# 风管三通排管
def duct_pipelist_main(flag_arr, pilist, block_arr, blockBase, PipeGroup_Type, ceilling, pipeBase):
    blocks_selection = blocks_select(pilist, block_arr, 5000)
    block_list_leave = copy.deepcopy(blocks_selection)
    print('旧管道是:', pilist)
    # plot_wall(pilist)
    slope_change_flag, pilist = Set_slope(pilist, PipeGroup_Type, ceilling)  # 为原始管道设置斜率
    linkedlist = LinkedList()  # 创建双向链表
    linkedlist.create_LinkedList(pilist)  # 初始化双向链表
    change_flag1 = pilist_avoidance(linkedlist, blocks_selection, blockBase, PipeGroup_Type, ceilling,
                                    pipeBase)  # 用于同一列中的管道避障
    new_pipeList = linkedlist.output_Arr()
    change_flag2, new_pipeList, position_list = Pi_bypass(new_pipeList, blocks_selection, blockBase, block_list_leave,
                                                          PipeGroup_Type, ceilling)
    change_flag = change_flag1 or change_flag2
    print('新管道是:', new_pipeList)
    return change_flag, slope_change_flag, new_pipeList, position_list


def double_update(new_pipeList, pipeBase, PipeGroupNo, PipeListNo, position_list, change_flag,
                  slope_change_flag):  # 将二通管道数组信息更新至管道类、增加连接关系、添加标球信息
    from Source.HJL.PipeClass import Pipe
    from Method import get_Unit_vector, rotation_2D
    import math

    # 添加标球信息
    cross_position_list = position_list[0]
    fail_position_list = position_list[1]
    for cross_postion in cross_position_list:
        pipeBase.yellowBallCoordinates.append(cross_postion)
    for fail_position in fail_position_list:
        pipeBase.blueBallCoordinates.append(fail_position)

    if change_flag or slope_change_flag:
        for PipeGroup in pipeBase._pipeGroups:
            if PipeGroup.pipeGroupNo == PipeGroupNo:
                for PipeList in PipeGroup._pipeLists:
                    if PipeList.pipeListNo == PipeListNo:
                        PipeList._pipes.clear()
                        for i in range(len(new_pipeList)):
                            pipe = Pipe()
                            pipe.uid = str(pipeBase.uniqueId)
                            pipeBase.uniqueId += 1
                            pipe.start = new_pipeList[i][0]
                            pipe.end = new_pipeList[i][1]
                            pipe.size.width = new_pipeList[i][2]
                            pipe.size.height = new_pipeList[i][3]
                            # todo cmf修改了这里 2023-12-7：添加了一些基础信息，方便设置检测机制表
                            # pipe.width = new_pipeList[i][2]
                            # pipe.height = new_pipeList[i][3]
                            # pipe.pipeInfo = [new_pipeList[i][0], new_pipeList[i][1], new_pipeList[i][2],
                            #                  new_pipeList[i][3]]
                            pipe.if_successful_avoid = new_pipeList[i][5]
                            # cmf
                            if new_pipeList[i][4] != None:
                                angle = new_pipeList[i][4]
                                tan_value1 = math.tan(angle)
                                tranform_2 = [1, tan_value1, 0]
                                tranform_2 = get_Unit_vector(tranform_2)
                                if new_pipeList[i][0][2] > new_pipeList[i][1][2]:
                                    tranform_3 = rotation_2D(tranform_2, -90)
                                else:
                                    tranform_3 = rotation_2D(tranform_2, 90)
                                if new_pipeList[i][0][2] > new_pipeList[i][1][2]:  # 若起点z坐标大于终点z坐标，第三个法向量指向z-
                                    tranform_1 = [0, 0, -1]
                                else:
                                    tranform_1 = [0, 0, 1]
                                temp1 = ','.join([str(tempFloat) for tempFloat in tranform_1])
                                temp2 = ','.join([str(tempFloat) for tempFloat in tranform_2])
                                temp3 = ','.join([str(tempFloat) for tempFloat in tranform_3])
                                location = ','.join([str(coordinate / 304.8) for coordinate in new_pipeList[i][0]])
                                pipe.transform = ';'.join([location, temp1, temp2, temp3])
                            PipeList._pipes.append(pipe)
    else:  # 若未发生避障或未设置斜率
        for PipeGroup in pipeBase._pipeGroups:
            if PipeGroup.pipeGroupNo == PipeGroupNo:
                for PipeList in PipeGroup._pipeLists:
                    if PipeList.pipeListNo == PipeListNo:
                        for i in range(len(new_pipeList)):
                            PipeList._pipes[i].start = new_pipeList[i][0]
                            PipeList._pipes[i].end = new_pipeList[i][1]

    # 增加连接关系
    for PipeGroup in pipeBase._pipeGroups:
        if PipeGroup.pipeGroupNo == PipeGroupNo:
            for pipeList in PipeGroup._pipeLists:
                if pipeList.pipeListNo == PipeListNo:
                    if len(pipeList._nodes) == 0:  # 该列为实体管道
                        if change_flag or slope_change_flag:  # 若该列管道发生改变，则全部使用默认类型的连接件
                            if len(pipeList._pipes) > 1:
                                for i in range(len(pipeList._pipes) - 1):
                                    PipeGroup.connectors.append(
                                        [[pipeList._pipes[i].uid, pipeList._pipes[i + 1].uid], 'Default', 'Default'])
                    else:  # 该列为点线
                        if change_flag:  # 若发生避障，则使用默认类型的连接件
                            if len(pipeList._pipes) > 1:
                                for i in range(len(pipeList._pipes) - 1):
                                    PipeGroup.connectors.append(
                                        [[pipeList._pipes[i].uid, pipeList._pipes[i + 1].uid], 'Default', 'Default'])
                        else:  # 若未发生避障，无论是否设置了斜率，则根据输入信息增加连接关系
                            for i in range(len(pipeList._pipes) - 1):
                                PipeGroup.connectors.append([[pipeList._pipes[i].uid, pipeList._pipes[i + 1].uid],
                                                             pipeList._nodes[i].familyName,
                                                             pipeList._nodes[i].symbolName])


# 由管道列标号,返回该列管道数组
def PipeListNo_to_PipeList(PipeGroupNo, PipeListNo, pipeBase):
    a = []
    for PipeGroup in pipeBase._pipeGroups:
        if PipeGroup.pipeGroupNo == PipeGroupNo:
            for pipeList in PipeGroup._pipeLists:
                if pipeList.pipeListNo == PipeListNo:
                    for pipe in pipeList._pipes:
                        # todo cmf2023-12-7修改 添加一个if_successful_avoid属性
                        a.append([pipe.start, pipe.end, pipe.size.width, pipe.size.height, pipe.angle,
                                  pipe.if_successful_avoid])
                        # cmf end
    return a


# 重构管道的起点
class DataAdjuster:
    def __init__(self, data):
        # 初始化数据
        self.data = data

    def compare_coordinates(self, coord1, coord2):
        # 比较坐标，如果Z轴不同，返回Z轴较大的坐标；如果Z轴相同，返回距离原点更近的坐标
        if coord1[2] != coord2[2]:
            return coord1 if coord1[2] >= coord2[2] else coord2
        else:
            distance1 = (coord1[0] ** 2 + coord1[1] ** 2) ** 0.5
            distance2 = (coord2[0] ** 2 + coord2[1] ** 2) ** 0.5
            return coord1 if distance1 <= distance2 else coord2

    def reverse_sublists(self):
        # 反转子列表
        reversed_data = []
        for pipe in reversed(self.data):
            pipe[0], pipe[1] = pipe[1], pipe[0]
            reversed_data.append(pipe)
        self.data = reversed_data

    def adjust_data(self):
        if len(self.data) == 1:
            # 如果数据长度为1，直接比较坐标并调整
            start_point = self.compare_coordinates(self.data[0][0], self.data[0][1])
            end_point = self.data[0][1] if start_point == self.data[0][0] else self.data[0][0]
            self.data[0][0], self.data[0][1] = start_point, end_point
        elif len(self.data) > 1:
            # 如果数据长度大于1，比较第一个和最后一个子列表的坐标，并根据需要反转子列表
            start_point_first = self.compare_coordinates(self.data[0][0], self.data[-1][1])
            if start_point_first == self.data[-1][1]:
                self.reverse_sublists()

        return self.data

    def reveresd_adjust_data(self, copy_pilist, copy_change_pilist):
        if len(self.data) == 1:
            # 如果数据长度为1，直接比较坐标并调整
            if copy_pilist[0] != copy_change_pilist[0]:
                # print("不一样")
                start_point = self.data[0][1]
                end_point = self.data[0][0]
                self.data[0][0], self.data[0][1] = start_point, end_point
            # else:
            #     print("一样")
        elif len(self.data) > 1:

            if copy_pilist[0][0] != copy_change_pilist[0][0]:
                self.reverse_sublists()
                print("多段，不一样")
            else:
                print("多段，一样")

        return self.data
